/**
 * @file SWO.c
 * @author windowsair
 * @brief SWO（Serial Wire Output）支持实现
 * @change: 2021-02-17: 添加基础功能
 * @version 0.2
 * @date 2021-02-17
 * @copyright Copyright (c) 2021
 */

/* 版权声明（略）*/

#include "DAP_config.h"
#include "DAP.h"
//#include "uart_modify.h" // 修改后的UART驱动p
#include "swo.h"            // SWO相关定义
#include "usbd_core.h"      // SWO相关定义
#include "dap_usb_handle.h" // SWO相关定义

#include "esp_err.h"
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "freertos/event_groups.h"
#include "driver/uart.h"
#include "esp_log.h"
static const char *TAG = "SWO";

EventGroupHandle_t kSwoThreadEventGroup; // SWO线程事件组
TaskHandle_t swo_task_handle;            // SWO线程句柄
// 仅当启用SWO流传输时检查DAP版本兼容性
#if (SWO_STREAM != 0)
#ifdef DAP_FW_V1
#error "DAP V1不支持SWO流传输!"
#endif
#endif

// UART模式SWO支持
#if (SWO_UART != 0)
#ifndef SWO_USART_PORT
#define SWO_USART_PORT UART_NUM_2 // 默认使用UART2
#endif 
#define SWO_USART_PIN GPIO_NUM_13

static uint8_t USART_Ready = 0U; // UART就绪标志
#endif                           /* (SWO_UART != 0) */

// 通用SWO配置（UART或Manchester模式）
#if ((SWO_UART != 0) || (SWO_MANCHESTER != 0))

#define SWO_STREAM_TIMEOUT pdMS_TO_TICKS(50) // 流传输超时时间
#define USB_BLOCK_SIZE     64U               // USB传输块大小
#define TRACE_BLOCK_SIZE   64U               // 跟踪数据块大小（2^n）

// SWO调试全局变量
extern volatile uint32_t kSWO_read_index; // SWO数据读取位置索引
extern volatile uint32_t kSWO_read_num;   // SWO待读取数据数量
/* ========== 跟踪状态全局变量 ========== */
static uint8_t TraceTransport = 0U;        // 传输方式 (0=禁用,1=DAP命令,2=WinUSB)
static uint8_t TraceMode = 0U;             // 跟踪模式 (DAP_SWO_OFF/UART/MANCHESTER)
static uint8_t TraceStatus = 0U;           // 跟踪状态 (DAP_SWO_CAPTURE_ACTIVE等)
static uint8_t TraceError[2] = { 0U, 0U }; // 错误标志（双缓冲）
static uint8_t TraceError_n = 0U;          // 当前使用的错误缓冲区索引

/* ========== 跟踪缓冲区管理 ========== */
static uint8_t kSwoTraceBuf[SWO_BUFFER_SIZE]; // 环形跟踪缓冲区
static volatile uint32_t TraceIndexI = 0U;    // 输入索引（新数据写入位置）
static volatile uint32_t TraceIndexO = 0U;    // 输出索引（数据读取位置）
static volatile uint8_t TraceUpdate;          // 缓冲区更新标志
static uint32_t TraceBlockSize;               // 当前跟踪块大小

/* ========== 时间戳支持 ========== */
#if (TIMESTAMP_CLOCK != 0U)
static volatile struct
{
    uint32_t index; // 缓冲区索引
    uint32_t tick;  // 时间戳
} TraceTimestamp;
#endif

/************************************UART驱动改造接口函数*************************************************************************** */
esp_err_t swo_uart_driver_install(uart_port_t uart_num, int rx_buffer_size, int tx_buffer_size, int event_queue_size, QueueHandle_t *uart_queue, int intr_alloc_flags);
esp_err_t swo_uart_read_bytes_async_swo(uint32_t index, uint32_t length);
int swo_uart_read_bytes(uart_port_t uart_num, void *buf, uint32_t length, TickType_t ticks_to_wait);
esp_err_t swo_uart_driver_delete(uart_port_t uart_num);
/************************************UART驱动改造接口函数*************************************************************************** */

/* ========== 函数声明 ========== */
static void ClearTrace(void);        // 清除跟踪数据和错误
static void ResumeTrace(void);       // 恢复跟踪捕获
static uint32_t GetTraceCount(void); // 获取可读数据量
static uint8_t GetTraceStatus(void); // 获取并清除错误状态

/* ========== 流传输支持 ========== */
#if (SWO_STREAM != 0)
volatile uint8_t kSwoTransferBusy = 0U; // USB传输忙标志
static uint32_t TransferSize;           // 当前传输大小
#endif

// =============================== UART模式实现 ===============================
#if (SWO_UART != 0)

/**
 * @brief 启用/禁用UART SWO模式
 * @param enable 启用标志 (1=启用, 0=禁用)
 * @return 1=成功, 0=失败
 */
uint32_t UART_SWO_Mode(uint32_t enable)
{
    USART_Ready = 0U; // 重置就绪标志
    uart_config_t uart_config = {
        .baud_rate = 74880,                   // 默认波特率
        .data_bits = UART_DATA_8_BITS,        // 8位数据
        .parity = UART_PARITY_DISABLE,        // 无校验
        .stop_bits = UART_STOP_BITS_1,        // 1位停止位
        .flow_ctrl = UART_HW_FLOWCTRL_DISABLE // 无流控
    };
        // 配置UART参数
        uart_param_config(SWO_USART_PORT, &uart_config);

    if (enable) {

        int ret=uart_set_pin(SWO_USART_PORT, -1, SWO_USART_PIN, -1, -1);
        ESP_LOGI(TAG, "uart_set_pin", ret);

        // 安装UART驱动（1024字节RX缓冲区）
        if (swo_uart_driver_install(SWO_USART_PORT, 1024, 0, 0, NULL, 0) != ESP_OK) {
            swo_uart_driver_delete(SWO_USART_PORT); // 失败时卸载驱动
            return 0;
        }
        ESP_LOGI(TAG, "UART SWO mode enabled");
    } else {
        swo_uart_driver_delete(SWO_USART_PORT); // 禁用时卸载驱动
    }
    return 1;
}

/**
 * @brief 配置UART SWO波特率
 * @param baudrate 请求的波特率
 * @return 实际设置的波特率（失败返回0）
 */
uint32_t UART_SWO_Baudrate(uint32_t baudrate)
{
    ESP_LOGI(TAG, "UART SWO baudrate1: %d", baudrate);
    size_t len;
    // 限制最大波特率
    if (baudrate > SWO_UART_MAX_BAUDRATE)
        baudrate = SWO_UART_MAX_BAUDRATE;

    // 若捕获处于活动状态，先暂停
    if (TraceStatus & DAP_SWO_CAPTURE_ACTIVE) {
        uart_get_buffered_data_len(SWO_USART_PORT, &len); // 更新已接收数据量
        TraceIndexI += len;
        uart_flush(SWO_USART_PORT); // 清空UART缓冲区
    }
    ESP_EARLY_LOGI("swo", "UART SWO baudrate2: %d", baudrate);
    // 设置新波特率
    uart_set_baudrate(SWO_USART_PORT, baudrate);
    USART_Ready = 1U; // 标记为就绪

    uint32_t baudrate_tmp;
    uart_get_baudrate(SWO_USART_PORT, &baudrate_tmp);
    ESP_LOGE("SWO", "SWO_Thread baudrate %d", baudrate_tmp);
    
    // 恢复捕获（如果之前处于活动状态）
    if (TraceStatus & DAP_SWO_CAPTURE_ACTIVE) {
        if (!(TraceStatus & DAP_SWO_CAPTURE_PAUSED)) {
            uint32_t index = TraceIndexI & (SWO_BUFFER_SIZE - 1U);                  // 环形缓冲区索引
            uint32_t remain = TRACE_BLOCK_SIZE - (index & (TRACE_BLOCK_SIZE - 1U)); // 剩余块大小
            TraceBlockSize = remain;                                                // 设置当前块大小
            swo_uart_read_bytes_async_swo(index, remain);                            // 启动异步读取
        }
    }
    return baudrate;
}

/**
 * @brief 控制UART SWO捕获状态
 * @param active 激活标志 (1=激活, 0=停用)
 * @return 1=成功, 0=失败
 */
uint32_t UART_SWO_Control(uint32_t active)
{
    size_t len;

    if (active) {
        if (!USART_Ready)
            return 0;        // UART未就绪
        TraceBlockSize = 1U; // 初始块大小
        if (swo_uart_read_bytes_async_swo(0, 1U) != ESP_OK)
            return 0; // 启动读取失败
    } else {
        // 停用时更新索引并清空缓冲区
        uart_get_buffered_data_len(SWO_USART_PORT, &len);
        TraceIndexI += len;
        uart_flush(SWO_USART_PORT);
    }
    return 1;
}

/**
 * @brief 启动UART数据捕获
 * @param index 缓冲区起始索引
 * @param num 要捕获的字节数
 */
static void UART_SWO_Capture(uint32_t index, uint32_t num)
{
    TraceBlockSize = num;                     // 设置块大小
    swo_uart_read_bytes_async_swo(index, num); // 异步读取
}

/**
 * @brief 获取UART待处理数据量
 * @return 待处理字节数
 */
static uint32_t UART_SWO_GetCount(void)
{
    size_t len;

    uart_get_buffered_data_len(SWO_USART_PORT, &len);

    return len;
}

#endif /* (SWO_UART != 0) */
// ===========================================================================

/* ========== 跟踪状态辅助函数 ========== */

/**
 * @brief 清除跟踪错误和数据
 */
static void ClearTrace(void)
{
// 流传输模式下中止进行中的传输
#if (SWO_STREAM != 0)
    if (TraceTransport == 2U && kSwoTransferBusy) {
        kSwoTransferBusy = 0U; // 标记传输结束
    }
#endif

    // 重置所有状态
    TraceError[0] = TraceError[1] = 0U;
    TraceError_n = 0U;
    TraceIndexI = TraceIndexO = 0U;

// 重置时间戳
#if (TIMESTAMP_CLOCK != 0U)
    TraceTimestamp.index = TraceTimestamp.tick = 0U;
#endif
}

/**
 * @brief 恢复跟踪捕获
 */
static void ResumeTrace(void)
{
    // 仅当处于暂停状态时恢复
    if (TraceStatus == (DAP_SWO_CAPTURE_ACTIVE | DAP_SWO_CAPTURE_PAUSED)) {
        uint32_t index_i = TraceIndexI;
        uint32_t index_o = TraceIndexO;

        // 检查缓冲区空间是否足够
        if ((index_i - index_o) < SWO_BUFFER_SIZE) {
            index_i &= SWO_BUFFER_SIZE - 1U; // 环形索引
            switch (TraceMode) {
#if (SWO_UART != 0)
                case DAP_SWO_UART:
                    TraceStatus = DAP_SWO_CAPTURE_ACTIVE; // 更新状态
                    UART_SWO_Capture(index_i, 1U);        // 启动捕获
                    break;
#endif
                    // 其他模式处理...
            }
        }
    }
}

/**
 * @brief 获取可读数据量
 * @return 可读字节数
 */
static uint32_t GetTraceCount(void)
{
    uint32_t count;
    if (TraceStatus == DAP_SWO_CAPTURE_ACTIVE) {
        // 活动状态下同步等待数据稳定
        do {
            TraceUpdate = 0U;
            count = TraceIndexI - TraceIndexO; // 基础计数
            // 按模式添加待处理数据
            switch (TraceMode) {
#if (SWO_UART != 0)
                case DAP_SWO_UART:
                    count += UART_SWO_GetCount();
                    break;
#endif
                    // 其他模式...
            }
            vTaskDelay(pdMS_TO_TICKS(10)); // 等待可能的更新
        } while (TraceUpdate);
    } else {
        count = TraceIndexI - TraceIndexO; // 非活动状态直接返回
    }
    return count;
}

/**
 * @brief 获取并清除错误状态
 * @return 当前状态（含错误标志）
 */
static uint8_t GetTraceStatus(void)
{
    uint8_t status;
    uint32_t n = TraceError_n;
    TraceError_n ^= 1U;                   // 切换错误缓冲区
    status = TraceStatus | TraceError[n]; // 合并状态和错误
    TraceError[n] = 0U;                   // 清除已读错误
    return status;
}

/**
 * @brief 设置错误标志
 * @param flag 要设置的错误标志位
 */
void SetTraceError(uint8_t flag)
{
    TraceError[TraceError_n] |= flag; // 设置当前错误缓冲区
}

/* ========== DAP命令处理函数 ========== */

/**
 * @brief 处理SWO传输方式命令
 * @return 响应数据长度和请求数据长度
 */
uint32_t SWO_Transport(const uint8_t *request, uint8_t *response)
{
    uint8_t transport = *request;
    uint32_t result = 0;
    
    // 仅在非活动状态允许更改
    if (!(TraceStatus & DAP_SWO_CAPTURE_ACTIVE)) {
        switch (transport) {
            case 0:
            case 1: // 禁用或DAP命令模式
#if (SWO_STREAM != 0)
            case 2: // WinUSB流模式
#endif
                TraceTransport = transport;
                result = 1;
                break;
            default:
                break;
        }
    }

    *response = result ? DAP_OK : DAP_ERROR;
    return (1 << 16) | 1; // 返回长度信息
}

/**
 * @brief 处理SWO模式命令
 */
uint32_t SWO_Mode(const uint8_t *request, uint8_t *response)
{
    uint8_t mode = *request;
    uint32_t result = 0;
    ESP_LOGI(TAG, "SWO_Mode: %d", mode);
    // 停用当前模式
    switch (TraceMode) {
#if (SWO_UART != 0)
        case DAP_SWO_UART:

            UART_SWO_Mode(0);
            break;
#endif
            // 其他模式处理...
    }

    // 启用新模式
    switch (mode) {
        case DAP_SWO_OFF: // 关闭模式
            result = 1;
            break;
#if (SWO_UART != 0)
        case DAP_SWO_UART: // UART模式
            result = UART_SWO_Mode(1);
            break;
#endif
            // 其他模式...
    }

    // 更新模式和状态
    TraceMode = result ? mode : DAP_SWO_OFF;
    TraceStatus = 0;
    *response = result ? DAP_OK : DAP_ERROR;
    return (1 << 16) | 1;
}

/**
 * @brief 处理SWO波特率命令
 */
uint32_t SWO_Baudrate(const uint8_t *request, uint8_t *response)
{
    uint32_t baudrate = *(uint32_t *)request; // 从请求中读取波特率

    // 按当前模式设置波特率
    switch (TraceMode) {
#if (SWO_UART != 0)
        case DAP_SWO_UART:
            baudrate = UART_SWO_Baudrate(baudrate);
            break;
#endif
        default:
            baudrate = 0; // 不支持的模式
    }

    // 设置失败时重置状态
    if (!baudrate)
        TraceStatus = 0;

    // 返回实际波特率
    *(uint32_t *)response = baudrate;
    return (4 << 16) | 4;
}

/**
 * @brief 处理SWO控制命令
 */
uint32_t SWO_Control(const uint8_t *request, uint8_t *response)
{
    uint8_t active = *request & DAP_SWO_CAPTURE_ACTIVE;
    uint32_t result = 0;
    ESP_LOGI(TAG, "SWO_Control: %d", active);
    // 仅当状态变化时处理
    if (active != (TraceStatus & DAP_SWO_CAPTURE_ACTIVE)) {
        if (active)
            ClearTrace(); // 激活前清除历史数据

        // 按模式控制捕获
        switch (TraceMode) {
#if (SWO_UART != 0)
            case DAP_SWO_UART:
                result = UART_SWO_Control(active);
                break;
#endif
                // 其他模式...
        }

        // 更新状态
        if (result) {
            TraceStatus = active;
#if (SWO_STREAM != 0)
            if (TraceTransport == 2) // WinUSB模式通知线程
                xEventGroupSetBits(kSwoThreadEventGroup, SWO_GOT_DATA);
#endif
        }
    } else {
        result = 1; // 状态未变化视为成功
    }

    *response = result ? DAP_OK : DAP_ERROR;
    return (1 << 16) | 1;
}

/**
 * @brief 处理SWO状态命令
 * @return 状态数据长度
 */
uint32_t SWO_Status(uint8_t *response)
{
    uint8_t status = GetTraceStatus(); // 获取状态
    uint32_t count = GetTraceCount();  // 获取数据量
    ESP_LOGI(TAG, "SWO_Status: %d", status);

    // 填充响应
    response[0] = status;
    response[1] = (uint8_t)(count);
    response[2] = (uint8_t)(count >> 8);
    response[3] = (uint8_t)(count >> 16);
    response[4] = (uint8_t)(count >> 24);

    return 5; // 返回数据长度
}

// 其他命令处理函数（略）...

/* ========== 流传输线程实现 ========== */
#if (SWO_STREAM != 0)

/**
 * @brief 传输完成回调
 */
void SWO_TransferComplete(void)
{
    TraceIndexO += TransferSize;                            // 更新输出索引
    kSwoTransferBusy = 0U;                                  // 清除忙标志
    ResumeTrace();                                          // 恢复捕获
    xEventGroupSetBits(kSwoThreadEventGroup, SWO_GOT_DATA); // 通知线程
}
void SWO_QueueTransfer(uint8_t *buf, uint32_t num);
/**
 * @brief SWO主处理线程
 */
void SWO_Thread()
{
    EventBits_t flags;
    uint32_t count, index, i, n;
    uint32_t index_i, index_o, remain;
    TickType_t timeout = portMAX_DELAY;

    kSwoThreadEventGroup = xEventGroupCreate();
    ESP_LOGI("SWO", "SWO_Thread start");
    /*miduo tmp add*/
    UART_SWO_Mode(1);
    swo_uart_read_bytes_async_swo(0,2);
    while (1) {
        /* 事件处理：
     * - SWO_GOT_DATA: 有待传输数据
     * - UART_GOT_DATA: UART接收完成
     * - SWO_ERROR_TIME_OUT: 超时或错误
     */
        flags = xEventGroupWaitBits(
            kSwoThreadEventGroup,
            SWO_GOT_DATA | UART_GOT_DATA | SWO_ERROR_TIME_OUT,
            pdTRUE,  // 自动清除事件标志
            pdFALSE, // 不等待所有标志
            timeout);

        /* UART数据处理 */
        if (flags & UART_GOT_DATA) {
            // 从UART读取数据到缓冲区
            swo_uart_read_bytes(SWO_USART_PORT, &kSwoTraceBuf[kSWO_read_index], kSWO_read_num, pdMS_TO_TICKS(20));
            ESP_LOGI(TAG, "SWO_Thread read %d bytes", kSWO_read_num);
            for(int i = 0; i < kSWO_read_num; i++)
            {
                ESP_LOGI(TAG, "SWO_Thread read %d byte: %d", i, kSwoTraceBuf[kSWO_read_index+i]);
            }
            // 更新索引
            index_o = TraceIndexO;
            TraceIndexI += TraceBlockSize;
            index_i = TraceIndexI;

// 更新时间戳
#if (TIMESTAMP_CLOCK != 0U)
            TraceTimestamp.tick = TIMESTAMP_GET();
            TraceTimestamp.index = index_i;
#endif

            // 计算下一个块大小
            remain = TRACE_BLOCK_SIZE - (index_i & (TRACE_BLOCK_SIZE - 1U));
            count = index_i - index_o; // 未处理数据量

            if (count <= (SWO_BUFFER_SIZE - remain)) {
                // 缓冲区足够，启动下次读取
                index_i &= SWO_BUFFER_SIZE - 1U;
                TraceBlockSize = remain;
                swo_uart_read_bytes_async_swo(index_i, remain);
            } else {
                // 缓冲区不足，暂停捕获
                TraceStatus = DAP_SWO_CAPTURE_ACTIVE | DAP_SWO_CAPTURE_PAUSED;
            }
            TraceUpdate = 1U; // 标记更新
        }

        /* 超时和状态处理 */
        if (!(TraceStatus & DAP_SWO_CAPTURE_ACTIVE)) {
            flags = SWO_ERROR_TIME_OUT;
            timeout = portMAX_DELAY;
        } else {
            timeout = pdMS_TO_TICKS(150);
        }

        /* 数据传输处理 */
        if (!kSwoTransferBusy) {
            count = GetTraceCount(); // 获取可读数据量
            if (count) {
                index = TraceIndexO & (SWO_BUFFER_SIZE - 1U); // 环形索引
                n = SWO_BUFFER_SIZE - index;                  // 连续空间大小

                if (count > n)
                    count = n; // 限制不超过连续空间

                // 非超时模式下对齐USB块
                if (!(flags & SWO_ERROR_TIME_OUT)) {
                    i = index & (USB_BLOCK_SIZE - 1U); // 当前块内偏移
                    if (i == 0) {
                        count &= ~(USB_BLOCK_SIZE - 1U); // 对齐块大小
                    } else {
                        n = USB_BLOCK_SIZE - i;       // 当前块剩余空间
                        count = (count >= n) ? n : 0; // 取整块或0
                    }
                }

                // 提交传输
                if (count) {
                    TransferSize = count;
                    kSwoTransferBusy = 1U;
                    SWO_QueueTransfer(&kSwoTraceBuf[index], count); // 通过USB传输
                }
            }
        }
    } // end while
}
#endif /* (SWO_STREAM != 0) */

/**
 * @brief SWO数据队列传输
 * @param buf 数据缓冲区指针，指向待传输的SWO数据
 * @param num 要传输的字节数，即待传输的SWO数据的长度
 * 该函数用于设置待发送的SWO数据指针和数据长度。
 */
void SWO_QueueTransfer(uint8_t *buf, uint32_t num)
{
    uint32_t baudrate;
    uart_get_baudrate(SWO_USART_PORT, &baudrate);
    ESP_LOGE("SWO", "SWO_Thread baudrate %d", baudrate);
    ESP_LOGI("SWO", "SWO_QueueTransfer: %d bytes", num);
    // for (int i = 0; i < num; i++) {
    //     ESP_LOGI("SWO", "%c", buf[i]);
    // }
    usbd_ep_start_write(0, SWO_IN_EP, buf, num);
    //ulTaskNotifyTake(pdTRUE, portMAX_DELAY); // 等待USB完成发送
    // // 设置待发送数据指针
    // swo_data_to_send = buf;
    // // 设置数据长度
    // swo_data_num = num;
}

void swo_init(void)
{
#if (SWO_STREAM != 0)

    // 创建JTAG任务(绑定到当前核心)
    BaseType_t res = xTaskCreatePinnedToCore(
        SWO_Thread,   // 任务函数
        "SWO_Thread", // 任务名称
        3 * 1024,     // 堆栈大小(4KB)
        NULL,         // 参数
        9,
        &swo_task_handle,     // 任务句柄
        esp_cpu_get_core_id() // 绑定到当前核心
    );
#endif /* (SWO_STREAM != 0) */
}
uint32_t SWO_ExtendedStatus(const uint8_t *request, uint8_t *response)
{
    uint8_t cmd = *request;       // 提取命令位
    uint32_t num = 0;             // 响应字节计数
    uint8_t *resp_ptr = response; // 响应指针
    ESP_LOGE(TAG, "SWO_ExtendedStatus: %d", cmd);
    // ====================== 状态标志处理 ======================
    if (cmd & 0x01U) {
        // 原子获取状态并清除错误
        uint8_t status = TraceStatus | TraceError[TraceError_n];
        TraceError[TraceError_n] = 0; // 清除当前错误

        *resp_ptr++ = status;
        num++;
    }

    // ====================== 数据量处理 ======================
    if (cmd & 0x02U) {
        // 原子获取数据量（避免索引变化）
        uint32_t count = TraceIndexI - TraceIndexO;

// 添加UART待处理数据（若启用）
#if (SWO_UART != 0)
        if (TraceMode == DAP_SWO_UART && TraceStatus) {
            size_t uart_pending;
            uart_get_buffered_data_len(SWO_USART_PORT, &uart_pending);
            count += uart_pending;
        }
#endif

        // 写入响应
        *resp_ptr++ = (uint8_t)(count);
        *resp_ptr++ = (uint8_t)(count >> 8);
        *resp_ptr++ = (uint8_t)(count >> 16);
        *resp_ptr++ = (uint8_t)(count >> 24);
        num += 4;
    }

// ====================== 时间戳处理 ======================
#if (TIMESTAMP_CLOCK != 0U)
    if (cmd & 0x04U) {
        uint32_t index, tick;
        const TickType_t max_retry = pdMS_TO_TICKS(10); // 最大重试时间

        // 带超时的稳定读取
        TickType_t start = xTaskGetTickCount();
        do {
            index = TraceTimestamp.index;
            tick = TraceTimestamp.tick;

            if (xTaskGetTickCount() - start > max_retry)
                break;     // 超时退出
            vTaskDelay(1); // 短暂让步
        } while (0); // 单次尝试（根据需求可改为循环）

        // 写入时间戳
        *resp_ptr++ = (uint8_t)(index);
        *resp_ptr++ = (uint8_t)(index >> 8);
        *resp_ptr++ = (uint8_t)(index >> 16);
        *resp_ptr++ = (uint8_t)(index >> 24);
        *resp_ptr++ = (uint8_t)(tick);
        *resp_ptr++ = (uint8_t)(tick >> 8);
        *resp_ptr++ = (uint8_t)(tick >> 16);
        *resp_ptr++ = (uint8_t)(tick >> 24);
        num += 8; // ✅ 修正为8字节
    }
#endif

    // ====================== 返回格式 ======================
    // 高16位：请求长度(1字节)，低16位：响应长度
    return (1 << 16) | num;
}

// Process SWO Data command and prepare response
//   request:  pointer to request data
//   response: pointer to response data
//   return:   number of bytes in response (lower 16 bits)
//             number of bytes in request (upper 16 bits)
uint32_t SWO_Data(const uint8_t *request, uint8_t *response)
{
    uint8_t status;
    uint32_t count;
    uint32_t index;
    uint32_t n, i;

    status = GetTraceStatus();
    count = GetTraceCount();
ESP_LOGE("SWO", "SWO_Data: %d bytes", count);
    if (TraceTransport == 1U) {
        n = (uint32_t)(*(request + 0) << 0) |
            (uint32_t)(*(request + 1) << 8);
        if (n > (DAP_PACKET_SIZE - 4U)) {
            n = DAP_PACKET_SIZE - 4U;
        }
        if (count > n) {
            count = n;
        }
    } else {
        count = 0U;
    }

    *response++ = status;
    *response++ = (uint8_t)(count >> 0);
    *response++ = (uint8_t)(count >> 8);

    if (TraceTransport == 1U) {
        index = TraceIndexO;
        for (i = index, n = count; n; n--) {
            i &= SWO_BUFFER_SIZE - 1U;
            *response++ = kSwoTraceBuf[i++];
        }
        TraceIndexO = index + count;
        ResumeTrace();
    }

    return ((2U << 16) | (3U + count));
}
#endif /* ((SWO_UART != 0) || (SWO_MANCHESTER != 0)) */
